#pragma once

#include "FastSerial.h"
#include <Arduino.h>
#include "AP_IMU.h"
#include "AP_DCM.h"
#include "Coord.h"
#include "myGps.h"

class Sensors
{
private:
	AP_IMU_INS *imu;
	AP_DCM *dcm;
	double calibX, calibY, calibZ;
	double battery;
	
	double vrate,lastAlt;//used for vrate
	long lastVrateTime;//used for vrate
	
public:
	MyGPS *gps;
	
	void init(AP_PeriodicProcess *scheduler, AP_IMU_INS &imu, AP_DCM &dcm, MyGPS &gps);
	void update();
	static void flash_leds(bool on);
	
	double getLat();
	double getLon();
	Coord getCoord();
	double getAlt();
	double getVrate();
	double getSpeed();
	double getRoll();
	double getPitch();
	double getYaw();
	double getHeading(); // true
	double getCourse();
	double getSlip(); //z accel
	double getAccel(); //y accel
	double getG(); //z accel
	double getBattery(){return battery;}
	
	double ax(){return imu->getIns()->ay();}
	double ay(){return imu->getIns()->ax();}
	double az(){return imu->getIns()->az();}
	double gx(){return imu->getIns()->gx();}
	double gy(){return imu->getIns()->gy();}
	double gz(){return imu->getIns()->gz();}
};
